#ifndef _MOTION_PLAN_H
#define _MOTION_PLAN_H

#include <string.h>
#include <termios.h>
#include <vector>

#include "../../include/Typedef.h"
#include "../../include/boat_datadef.h"

using namespace std;

typedef struct
{
	int DotNum;
	int TrackNum;
	int ReturnFlag;
	int TrackFlag;
	int FirstFlag;
	float deltat;
	float setVel;
	float eta;
	float switchlen;
	float maxSpeed;
	float firstphi;
	float FirstDotAngle;

	float TrajectoryUnit[50][2];
	float TargetPos[2];
}TrackingPlanParam;


class MotionPlan
{
public:
	MotionPlan();
	anchor_mission_control    tracking_task;

	flash_model_data actionModelData;

	actuator_control actionActuatorControl;
	headvel_control  actionHeadvelControl;
	trajectory_mission_control actionTrajectoryMissionControl;
	int _precmd = 0;
	TrackingPlanParam  dotTrackingParam;
	TrackingPlanParam  lineTrackingParam;

	float trackline[50][2];
	float lineunit[50][2];

	bool globalFirstFlag;

	double globalMapPos[2];

	int WaitTime, WaitTimeNum, WaitFlag;

	void GlobalMapFlash(TransState boatstate, vector<vector<int> >localmap);

	void TrajecotryGet(TransState boatstate, anchor_mission_control target);

	void TrajectoryTrackingDirect(TransState boatstate);

};

#endif
